18. Augmentation Assignment 2
Solution
Here's my process:
- I added two noise values to the mean of the augmented state. The mean of the noise values is zero, so I set these numbers to zero.
- I filled the augmented covariance matrix with zeros.
-
Then I set the top left block of the augmented covariance matrix using the function
topLeftcorner
from the cheat sheet.
4.I manually put the variances into the lower right block of the augmented matrix. This 2x2 block corresponds to the matrix Q .
The rest is exactly the same as before, except that I created more sigma points this time.
You can find my solution in the code editor below. Feel free to play with it!
Start Quiz:
#include "Dense"
#include "ukf.h"
using Eigen::MatrixXd;
int main() {
// Create a UKF instance
UKF ukf;
/**
* Programming assignment calls
*/
MatrixXd Xsig_aug = MatrixXd(7, 15);
ukf.AugmentedSigmaPoints(&Xsig_aug);
return 0;
}
#include <iostream>
#include "ukf.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
UKF::UKF() {
Init();
}
UKF::~UKF() {
}
void UKF::Init() {
}
/**
* Programming assignment functions:
*/
void UKF::AugmentedSigmaPoints(MatrixXd* Xsig_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// Process noise standard deviation longitudinal acceleration in m/s^2
double std_a = 0.2;
// Process noise standard deviation yaw acceleration in rad/s^2
double std_yawdd = 0.2;
// define spreading parameter
double lambda = 3 - n_aug;
// set example state
VectorXd x = VectorXd(n_x);
x << 5.7441,
1.3800,
2.2049,
0.5015,
0.3528;
// create example covariance matrix
MatrixXd P = MatrixXd(n_x, n_x);
P << 0.0043, -0.0013, 0.0030, -0.0022, -0.0020,
-0.0013, 0.0077, 0.0011, 0.0071, 0.0060,
0.0030, 0.0011, 0.0054, 0.0007, 0.0008,
-0.0022, 0.0071, 0.0007, 0.0098, 0.0100,
-0.0020, 0.0060, 0.0008, 0.0100, 0.0123;
// create augmented mean vector
VectorXd x_aug = VectorXd(7);
// create augmented state covariance
MatrixXd P_aug = MatrixXd(7, 7);
// create sigma point matrix
MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
/**
* Student part begin
*/
// create augmented mean state
x_aug.head(5) = x;
x_aug(5) = 0;
x_aug(6) = 0;
// create augmented covariance matrix
P_aug.fill(0.0);
P_aug.topLeftCorner(5,5) = P;
P_aug(5,5) = std_a*std_a;
P_aug(6,6) = std_yawdd*std_yawdd;
// create square root matrix
MatrixXd L = P_aug.llt().matrixL();
// create augmented sigma points
Xsig_aug.col(0) = x_aug;
for (int i = 0; i< n_aug; ++i) {
Xsig_aug.col(i+1) = x_aug + sqrt(lambda+n_aug) * L.col(i);
Xsig_aug.col(i+1+n_aug) = x_aug - sqrt(lambda+n_aug) * L.col(i);
}
/**
* Student part end
*/
// print result
std::cout << "Xsig_aug = " << std::endl << Xsig_aug << std::endl;
// write result
*Xsig_out = Xsig_aug;
}
/**
* expected result:
* Xsig_aug =
* 5.7441 5.85768 5.7441 5.7441 5.7441 5.7441 5.7441 5.7441 5.63052 5.7441 5.7441 5.7441 5.7441 5.7441 5.7441
* 1.38 1.34566 1.52806 1.38 1.38 1.38 1.38 1.38 1.41434 1.23194 1.38 1.38 1.38 1.38 1.38
* 2.2049 2.28414 2.24557 2.29582 2.2049 2.2049 2.2049 2.2049 2.12566 2.16423 2.11398 2.2049 2.2049 2.2049 2.2049
* 0.5015 0.44339 0.631886 0.516923 0.595227 0.5015 0.5015 0.5015 0.55961 0.371114 0.486077 0.407773 0.5015 0.5015 0.5015
* 0.3528 0.299973 0.462123 0.376339 0.48417 0.418721 0.3528 0.3528 0.405627 0.243477 0.329261 0.22143 0.286879 0.3528 0.3528
* 0 0 0 0 0 0 0.34641 0 0 0 0 0 0 -0.34641 0
* 0 0 0 0 0 0 0 0.34641 0 0 0 0 0 0 -0.34641
*/
#ifndef UKF_H
#define UKF_H
#include "Dense"
class UKF {
public:
/**
* Constructor
*/
UKF();
/**
* Destructor
*/
virtual ~UKF();
/**
* Init Initializes Unscented Kalman filter
*/
void Init();
/**
* Student assignment functions
*/
void GenerateSigmaPoints(Eigen::MatrixXd* Xsig_out);
void AugmentedSigmaPoints(Eigen::MatrixXd* Xsig_out);
void SigmaPointPrediction(Eigen::MatrixXd* Xsig_out);
void PredictMeanAndCovariance(Eigen::VectorXd* x_pred,
Eigen::MatrixXd* P_pred);
void PredictRadarMeasurement(Eigen::VectorXd* z_out,
Eigen::MatrixXd* S_out);
void UpdateState(Eigen::VectorXd* x_out,
Eigen::MatrixXd* P_out);
};
#endif // UKF_H
expected result:
Xsig_aug =
5.7441 5.85768 5.7441 5.7441 5.7441 5.7441 5.7441 5.7441 5.63052 5.7441 5.7441 5.7441 5.7441 5.7441 5.7441
1.38 1.34566 1.52806 1.38 1.38 1.38 1.38 1.38 1.41434 1.23194 1.38 1.38 1.38 1.38 1.38
2.2049 2.28414 2.24557 2.29582 2.2049 2.2049 2.2049 2.2049 2.12566 2.16423 2.11398 2.2049 2.2049 2.2049 2.2049
0.5015 0.44339 0.631886 0.516923 0.595227 0.5015 0.5015 0.5015 0.55961 0.371114 0.486077 0.407773 0.5015 0.5015 0.5015
0.3528 0.299973 0.462123 0.376339 0.48417 0.418721 0.3528 0.3528 0.405627 0.243477 0.329261 0.22143 0.286879 0.3528 0.3528
0 0 0 0 0 0 0.34641 0 0 0 0 0 0 -0.34641 0
0 0 0 0 0 0 0 0.34641 0 0 0 0 0 0 -0.34641